#include "mymath.h"



/*低通滤波
**scale_factor----范围0-1
*/
void LowPassFilter(float_t *value, float_t scale_factor)  
{
    static float valueLast = 0.0f;
    *value = (1-scale_factor) * valueLast + scale_factor * (*value);
    valueLast = *value;
}

float_t PIDCalculate(PID_Typedef *pidHandle, double_t SetValue, double_t ActualValue)
{
    double_t Error = SetValue - ActualValue;

//    pidHandle->I += pidHandle->Ki * Error * pidHandle->Ts;
    pidHandle->I += pidHandle->Ki * Error;

    // 积分限制幅度
    if(pidHandle->I>pidHandle->Imax)
    {
    	pidHandle->I = pidHandle->Imax;
    }
    else if(pidHandle->I<pidHandle->Imin)
    {
    	pidHandle->I = pidHandle->Imin;
    }

    double_t P = pidHandle->Kp * Error;                                           // P = Kp * Error;
    double_t D = pidHandle->Kd * (Error - pidHandle->last_Error) / pidHandle->Ts; // D = Kd * (Error - last_Error) / Ts

    float_t Uo = P + pidHandle->I + D; // 输出Uout = P + I + D

    // 存储当前误差
    pidHandle->last_Error = Error;

    // 限幅处理
    if (Uo > pidHandle->Umax)
    {
        Uo = pidHandle->Umax;
    }
    else if (Uo < pidHandle->Umin)
    {
        Uo = pidHandle->Umin;
    }
    return Uo;
}

void bubble_sort(uint16_t arr[], uint16_t n) {
    for (uint16_t i = 0; i < n - 1; i++) {
        for (uint16_t j = 0; j < n - i - 1; j++) {
            if (arr[j] > arr[j + 1]) {
                // 交换元素
                uint16_t temp = arr[j];
                arr[j] = arr[j + 1];
                arr[j + 1] = temp;
            }
        }
    }
}
